// state.c - state initialization and update
// -----------------------------------------

#include <math.h>
#include <stdio.h>
#include "state.h"
#include "frame.h"
#include "mmath.h"

double** qum;		// quaternion update matrix
double** scul;		// sculling matrix
double*  pq;		// previous quaternion
double*  pvn;		// previous velocity in the navigation frame
double*  t3_1;		// 1st temporary 3-vector
double*  t3_2;		// 2nd temporary 3-vector
double*  euler;		// euler angle matrix

void _init() {
	double dummy[3] = {0, 0, 0};
	
	qum   = dmatalloc(4, 4);
	scul  = sculling_matrix(dummy);
	pq    = dvecalloc(4);
	pvn   = dvecalloc(3);
	t3_1  = dvecalloc(3);
	t3_2  = dvecalloc(3);
	euler = dvecalloc(3);
}

void _destroy() {
	dmatfree(qum, 4);
	dmatfree(scul, 3);
	dvecfree(pq);
	dvecfree(pvn);
	dvecfree(t3_1);
	dvecfree(t3_2);
	dvecfree(euler);
}

void _state_init(STATE *s, double rx, double ry, double rz, double vx, double vy, double vz, double phi, double theta, double psi) {
	s->rn = dvecalloc(3);
	s->rn[0] = rx;
	s->rn[1] = ry;
	s->rn[2] = rz;
	
	s->vn = dvecalloc(3);
	s->vn[0] = vx;
	s->vn[1] = vy;
	s->vn[2] = vz;
	
	s->Cbn = dmatalloc(3, 3);
	euler[0] = phi;
	euler[1] = theta;
	euler[2] = psi;
	s->Cbn = euler_angles_to_body_to_nav_matrix(euler);
	
	s->q = dvecalloc(4);
	s->q = body_to_nav_matrix_to_quaternion(s->Cbn);
}

void _state_destroy(STATE *s) {
	dvecfree(s->rn);
	dvecfree(s->vn);
	dvecfree(s->q);
	dmatfree(s->Cbn, 3);
}

void _state_update(STATE *s, double* dtheta, double* dv, double dt) {
	// dtheta and dv initially in body frame
	_vscale3(dtheta, dt);		// multiply d(theta_ib^b) by dt to get change in angular position
	_vscale3(dv, dt);			// multiply d(v_b^b) by dt to get change in velocity
	
	// upate quaternion using Eskin's method
	_quaternion_update_matrix_shin(qum, dtheta);
	_vcopy4(s->q, pq);
	_mvmult4(s->q, qum, pq);
	
	// update body-to-navigation matrix
	_quaternion_to_body_to_nav_matrix_shin(s->Cbn, s->q);
	
	// update sculling matrix
	_sculling_matrix(scul, dtheta);
	
	// convert dv from body frame to navigation frame
	_mvmult3(t3_1, scul, dv);
	_mvmult3(dv, s->Cbn, t3_1);
	
	// update velocity
	_vcopy3(s->vn, pvn);
	_vadd3(s->vn, pvn, dv);
	
	// update position
	_vadd3(t3_1, pvn, s->vn);
	_vscale3(t3_1, dt*0.5);
	_vadd3(t3_2, s->rn, t3_1);
	_vcopy3(t3_2, s->rn);
}

void _state_print(STATE *s) {
	printf("------------------------------------------------------\n");
	printf("rn = [ %lf %lf %lf ]\n\n", s->rn[0], s->rn[1], s->rn[2]);
	printf("vn = [ %lf %lf %lf ]\n\n", s->vn[0], s->vn[1], s->vn[2]);
	printf("q  = [ %lf %lf %lf %lf ]\n\n", s->q[0], s->q[1], s->q[2], s->q[3]);
	printf("C  = [ %lf %lf %lf\n       %lf %lf %lf\n       %lf %lf %lf ]\n\n", s->Cbn[0][0], s->Cbn[0][1], s->Cbn[0][2], s->Cbn[1][0], s->Cbn[1][1], s->Cbn[1][2], s->Cbn[2][0], s->Cbn[2][1], s->Cbn[2][2]);
}

void _state_log_init(FILE* f) {
	fprintf(f, "t\tx\ty\tz\tvx\tvy\tvz\tphi\ttheta\tpsi\n");
}

void _state_log(FILE* f, double t, STATE *s) {
	fprintf(f, "%lf\t", t);
	fprintf(f, "%lf\t%lf\t%lf\t", s->rn[0], s->rn[1], s->rn[2]);
	fprintf(f, "%lf\t%lf\t%lf\t", s->vn[0], s->vn[1], s->vn[2]);
	_body_to_nav_matrix_to_euler_angles(euler, s->Cbn);
	fprintf(f, "%lf\t%lf\t%lf\n", euler[0], euler[1], euler[2]);
}
